/**
 * @file route.c
 * @author sgfz
 * @brief 平台间完整路线
 * @version 0.1
 * @date 2024-09-12
 *
 * @copyright Copyright (c) 2024
 *
 */
#include "route.h"
#include "reset.h"
#include "posture.h"
#include "trace.h"
#include "bsp_adc.h"
#include "turn.h"
#include "bsp_servo.h"
#include "bsp_sensor.h"
#include "bsp_SysTick.h"

extern int cnt_hdl;
extern int cnt_hdr;

/**
 * @brief 门2转门1
 *
 */
void door2_door1(void)
{
    // 后退
    run_delay(-40, -40, 150);
    // 掉头
    JD1_2_TurnAround();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 100);
    speed_down(100, 60);
    // 左转进门1
    TurnLeft_155();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 120);
    speed_down(120, 60);
    // 停止线
    Stop_Line();
}
/**
 * @brief 门1转门4
 *
 */
void door1_door4(void)
{
    // 后退
    run_delay(-40, -40, 150);
    // 掉头
    JD1_2_TurnAround();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 100);
    speed_down(100, 60);
    // 右转
    TurnRight_90_Ldetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 210);
    speed_down(210, 60);
    // 右转
    TurnRight_90_hdrDetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 110);
    speed_down(110, 60);
    // 停止线
    Stop_Line();
}
/**
 * @brief 门4转门3
 *
 */
void door4_door3(void)
{
    // 后退
    run_delay(-40, -40, 150);
    // 掉头
    JD1_2_TurnAround();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 100);
    speed_down(100, 60);
    // 左转
    TurnLeft_155_hdlDetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 110);
    speed_down(110, 60);
    // 停止线
    Stop_Line();
}
/**
 * @brief 平台1到平台2
 */
void Tai1_Tai2(void)
{
    // 下台
    down_pt1_6();
    // 加速
    speed_up(50, 125);
    speed_down(125, 50);
    // 矫正
    Reset(50, 50);
    // 过桥
    Bridge_Travel();
    // 加速
    speed_up(50, 120);
    speed_down(120, 50);
    // 上台2
    UP_Tai1_6();
}
/**
 * @brief 台2到景点2到台4
 *
 */
void Tai2_jd2_Tai4(void)
{
    // 下台2
    down_pt1_6();
    // 拉前铲
    Front_mid();
    // 右转
    TurnRight_45_TXS();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 90);
    speed_down(90, 60);
    // 梯形山
    T_Moutain_Slow();
    // 加速
    speed_up(60, 90);
    speed_down(90, 60);
    // 检测左转
    TurnLeft_45_Longline();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 210);
    Reset(20, 210);
    speed_down(210, 60);
    // 检测左转进景点2
    TurnLeft_inJD2();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 170);
    speed_down(170, 60);
    // 撞景点2
    Crash_JD(2);
    // 掉头
    JD1_2_TurnAround();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 150);
    speed_down(150, 60);
    // 左转上台2
    TurnLeft_155();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 100);
    speed_down(100, 60);
    // 降前铲
    Front_down();
    // 上台2
    UP_Tai1_6();
}
/**
 * @brief 台4到台3
 *
 */
void Tai4_Tai3(void)
{
    // 下台4
    down_pt1_6();
    // 加速
    speed_up(60, 205);
    Reset(340, 205);
    speed_down(205, 60);
    // 上台3
    UP_Tai1_6();
}
/**
 * @brief 台3到景点1转出加速 未进门区 一二趟通用
 *
 */
void Tai3_jd1_OUT(void)
{
    // 下台3
    down_pt1_6();
    // 加速
    speed_up(60, 195);
    speed_down(195, 60);
    // 左转进景点1
    TurnLeft_inJD1();
    // 矫正
    Reset(80, 60);
    // 加速
    speed_up(60, 200);
    Reset(20, 200);
    speed_down(200, 60);
    // 撞景点1
    Crash_JD(1);
    // 掉头
    JD1_2_TurnAround();
    // 矫正
    Reset(70, 60);
    // 加速
    speed_up(60, 200);
    speed_down(200, 90);
}
/**
 * @brief 台3到景点1到门1
 *
 */
void Tai3_jd1_door1(void){
    // 出景点进门区前
    Tai3_jd1_OUT();
    TurnLeft_inDoor1();
}
/**
 * @brief 台3到景点1到门2
 *
 */
void Tai3_jd1_door2(void)
{
    // 出景点进门区前
    Tai3_jd1_OUT();
    // 顺拐进门2
    Small_Turn_inDoor2();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 100);
    speed_down(100, 60);
    // 停止线
    Stop_Line();
}
/**
 * @brief 台3到景点1到门3
 *
 */
void Tai3_jd1_door3(void){
    // 出景点进门区前
    Tai3_jd1_OUT();
    //顺转到直线
    Small_Turn_inline();

    speed_up(60, 210);
    Reset(20, 210);
    speed_down(210, 60);
    TurnRight_155_door3();

    Reset(60, 60);
    speed_up(60, 140);
    speed_down(140, 50);
}
/**
 * @brief 台3到景点1到门4
 *
 */
void Tai3_jd1_door4(void){
    // 出景点进门区前
    Tai3_jd1_OUT();
    //顺转到直线
    Small_Turn_inline();

    speed_up(60, 210);
    Reset(20, 210);
    speed_down(210, 60);

    TurnRight_90_hdrDetect();
    Reset(60, 60);
    speed_up(60, 140);
    speed_down(140, 50);
}
/**
 * @brief 门1到台5
 *
 */
void door1_Tai5(void)
{
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 125);
    speed_down(125, 60);
    // 左转
    TurnLeft_Door1_inCross();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 160);
    speed_down(160, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 170);
    speed_down(170, 60);
    // 转进景点5
    TurnLeft_45_inTai5();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 190);
    speed_down(190, 60);
    // 放前铲
    Front_down();
    // 上台5
    UP_Tai1_6();
}
/**
 * @brief 门2到台5
 *
 */
void door2_Tai5(void)
{
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 180);
    Reset(30, 180);
    speed_down(180, 60);
    // 转进景点5
    TurnLeft_45_inTai5();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 180);
    speed_down(180, 60);
    // 放前铲
    Front_down();
    // 上台5
    UP_Tai1_6();
}
/**
 * @brief 门3到台5
 *
 */
void door3_Tai5(void)
{
    // 矫正
    Reset(50, 50);
    // 加速
    speed_up(50, 90);
    speed_down(90, 50);
    // 左转
    TurnLeft_155_atCross();
    // 加速
    speed_up(60, 175);
    speed_down(175, 60);
    // 转进景点5
    TurnLeft_45_inTai5();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 180);
    speed_down(180, 60);
    // 放前铲
    Front_down();
    // 上台5
    UP_Tai1_6();
}
/**
 * @brief 门4到台5
 *
 */
void door4_Tai5(void)
{
    // 矫正
    Reset(50, 50);
    // 加速
    speed_up(50, 140);
    speed_down(140, 50);
    // 左转
    TurnLeft_90_Rdetect();
    // 矫正
    Reset(60, 50);
    // 加速
    speed_up(50, 200);
    speed_down(200, 60);
    // 放前铲
    Front_down();
    // 上台5
    UP_Tai1_6();
}
/**
 * @brief 台5到景点5到台7
 *
 */
void Tai5_jd5_Tai7(void)
{
    // 下台5
    down_pt1_6();
    Front_mid();
    // 加速
    speed_up(50, 195);
    speed_down(195, 50);
    // 左转
    TurnLeft_90_Rdetect();
    // 矫正
    Reset(50, 50);
    // 右转进景点5
    TurnRightJD_90();
    // 撞景点5
    Crash_JD(5);
    // 左转
    TurnLeft_Place_90();
    // 矫正
    Reset(60, 50);
    // 左转
    TurnLeft_90_Ldetect();
    // 矫正
    Reset(60, 50);
    // 加速
    speed_up(50, 150);
    speed_down(150, 50);
    // 梯形山
    T_Moutain_Fast();
    // 加速
    speed_up(50, 95);
    speed_down(95, 50);
    // 右转
    TurnRight_90_Rdetect();
    // 矫正
    Reset(60, 60);
    // 7号平台下桥洞
    speed_up(60, 140);
    speed_down(140, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 140);
    speed_down(140, 60);
    // 梯形山
    T_Moutain_Slow();
    // 加速
    speed_up(60, 145);
    speed_down(145, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 矫正
    Reset(80, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 130);
    speed_down(130, 60);
    // 长波浪板
    IN_Long_BLB();
    // 上台7
    UP_Tai7();
}
/**
 * @brief 台7到台8
 *
 */
void Tai7_Tai8(void)
{
    // 下坡
    Down_Tai7();
    // 长波浪板
    OUT_Long_BLB();
    // 加速
    speed_up(60, 180);
    speed_down(180, 60);
    // 梯形山
    T_Moutain_Slow();
    // 加速
    speed_up(60, 150);
    speed_down(150, 60);
    // 上台8
    UP_Tai8();
}
/**
 * @brief 台8到景点3
 *
 */
void Tai8_jd3(void)
{
    // 下台8
    Down_Tai8();
    // 加速
    speed_up(50, 110);
    speed_down(110, 50);
    // 左转
    TurnLeft_155();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 175);
    speed_down(175, 50);
    // 左转
    TurnLeft_155();
    // 过衔接平台桥洞
    speed_up(60, 120);
    Reset(60, 120);
    speed_down(120, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 加速
    speed_up(60, 140);
    speed_down(140, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 过高平台桥洞
    Reset(200, 70);
    // 加速
    speed_up(60, 130);
    speed_down(130, 60);
    // 右转进景点3
    TurnRightJD_90();
    // 撞景点3
    Crash_JD(3);
    // 左转
    TurnLeft_Place_90();
    // 矫正
    Reset(60, 60);
    // 右转
    TurnRight_90_Rdetect();
}
/**
 * @brief 景点3到景点4
 */
void jd3_jd4(void)
{
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 190);
    speed_down(190, 120);
    speed_up(120, 170);
    speed_down(170, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 矫正
    Reset(70, 60);
    // 右转进景点4
    TurnRightJD_90();
    // 矫正
    Reset(50, 60);
    // 撞景点
    Crash_JD(4);
    // 右转
    TurnRight_Place_90();
    // 矫正
    Reset(70, 60);
}
/**
 * @brief 景点4到门1回家
 *
 */
void jd4_door1_Home(void)
{
    // 检测路口
    Detect_OUT_jd4();
    // 矫正
    Reset(100, 70);
    // 加速
    speed_up(70, 180);
    speed_down(180, 60);
    // 右转
    TurnRight_90_Ldetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 140);
    speed_down(140, 60);
    // 转入波浪板
    door12_Turn_BLB();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 80);
    speed_down(80, 60);
    // 过波浪板
    Pass_Short_BLB();
    // 右转
    TurnRight_45_Tai1();
    // 矫正
    Reset(60, 60);
    // 上台1
    UP_Tai1();
}
/**
 * @brief 景点4到门2回家
 *
 */
void jd4_door2_Home(void)
{
    // 右转
    TurnRight_45_inCross();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 165);
    speed_down(165, 60);
    // 左转
    TurnLeft_155_atCross();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 150);
    speed_down(150, 60);
    // 右转大角度
    TurnRight_155_outCross();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 140);
    speed_down(140, 60);
    // 转入波浪板
    door12_Turn_BLB();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 80);
    speed_down(80, 60);
    // 过波浪板
    Pass_Short_BLB();
    // 右转
    TurnRight_45_Tai1();
    // 矫正
    Reset(60, 60);
    // 上台1
    UP_Tai1();
}
/**
 * @brief 景点4到门3回家
 *
 */
void jd4_door3_Home(void)
{
    // 右转
    TurnRight_45_inCross();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 210);
    Reset(30, 210);
    speed_down(210, 60);
    // 左转
    TurnLeft_155_atCross();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 135);
    speed_down(135, 60);
    // 门34转波浪板
    door34_Turn_BLB();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 80);
    speed_down(80, 60);
    // 过波浪板
    Pass_Short_BLB();
    // 右转
    TurnRight_45_Tai1();
    // 矫正
    Reset(60, 60);
    // 上台1
    UP_Tai1();
}
/**
 * @brief 景点4到门4回家
 *
 */
void jd4_door4_Home(void)
{
    // 右转
    TurnRight_45_inCross();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 160);
    speed_down(160, 60);
    // 右转
    TurnRight_90_Rdetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 165);
    speed_down(165, 60);
    // 左转大角度冲门4
    TurnLeft_155_door4();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 180);
    speed_down(180, 60);
    // 左转
    TurnLeft_90_Ldetect();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 135);
    speed_down(135, 60);
    // 转波浪板
    door34_Turn_BLB();
    // 矫正
    Reset(60, 60);
    // 加速
    speed_up(60, 80);
    speed_down(80, 60);
    // 过波浪板
    Pass_Short_BLB();
    // 右转
    TurnRight_45_Tai1();
    // 矫正
    Reset(60, 60);
    // 上台1
    UP_Tai1();
}
/********************第二趟********************* */
